#include <iostream>
#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>

#include "facedir.h"

#include "other_functions.h"

#include "view.h"
#include "model.h"
#include "createmodel.h"
#include "cvfundam2.h"

#include "options.h"

#include <boost/thread/locks.hpp>


void CFacedir::operator()(int i)
{
    while (!quit)
    {
	//////////////////////////////
	synchro3->wait();

	{
	    //boost::mutex::scoped_lock lock(m_mutex);
	    //anulamos los puntos que se pasen del angulo correspondiente
	    view[i].getNotHiden();
	    view[i].tracker(reader.img(i), cvSize(61, 61));
	    view[i].display_track(reader.img(i), backup[2+i]);
	    view[i].RANSAC_posit(&ransac_global_params);
	}
	//////////////////////////////
	synchro2->wait();
	if (i==0)
	    facedir_merge();

	//////////////////////////////
	synchro2->wait();
	view[i].RANSAC_results(reader.img(i));

	//////////////////////////////
	synchro3->wait();
    }    
}


void CFacedir::facedir_merge()
{
    int win=0, lost;

    if(0)
    {
	if (view[0].inliers > view[1].inliers)
	    win = 0;
	else if (view[1].inliers > view[0].inliers)
	    win = 1;
	else
	{
	    if (view[0].error > view[1].error)
		win = 0;
	    else
		win = 1;
	}
	lost = 1-win;

	view[win].calcPose(view[win].maskout);
	view[lost].ChangeView(view[win]);
    }
    else 
    {
	//media de los vectores
	float suma = view[0].inliers + view[1].inliers;
	float distR, distT;

	view[0].calcPose(view[0].maskout);
	view[1].calcPose(view[1].maskout);
	
	view_0_aux.ChangeView(view[1]);

	distT = sqrt(distanceT2(view[0], view_0_aux));
	distR = distanceR(view[0], view_0_aux);
	printf("dist R/T: %f, %f\n", distR*180.0/CV_PI, distT);

	if (0)//distT > MAX_DIST_T || distR > MAX_DIST_R)
	{
	}
	else
	{
	}

	cvAddWeighted(view[0].R, (float)view[0].inliers/suma, 
		      view_0_aux.R, (float)view[1].inliers/suma, 
		      0, view[0].R);
	cvAddWeighted(view[0].T, (float)view[0].inliers/suma, 
		      view_0_aux.T, (float)view[1].inliers/suma, 
		      0, view[0].T);
	
	view[1].ChangeView(view[0]);
    }
}


int CFacedir::main()
{
    int i;
    /***********/
    //lanza el hilo de lectura de las imágenes

    fout = NULL;
    videofile =0;

    //lanza el hilo de lectura de las imágenes
    reader.set_image_size(1100, 600);
    reader.start();
    reader.consumer();
    cvShowImage(window[0], reader.img(0));
    cvShowImage(window[1], reader.img(1));
    frame2 = reader.head(0)->nframe;    
    
    printf("Tamaño de la imagen: %d x %d\n",reader.image_size().width,reader.image_size().height);

    readCalibration(reader.get_pathbasename()+".cal" );
    view[0].linkmodel(model, &calibration_params[0]);
    view[1].linkmodel(model, &calibration_params[1]);

    //read 3D model file, if -r used at command line
    if (filemodel.size())
    {
	if (!ReadModel(basefilename, reader.get_ROI()))
	{
	    printf("Error. El modelo no es correcto.\n");
	    exit(-1);
	}
    }
    else
    {
	int npoints;
	//creates the model automatecaly
	npoints = do_Estereo(reader.get_last_used()->img, &view[0].points2d, &view[1].points2d, &model.points3d, &model.angY, 
			     view[0].calibration, view[1].calibration, &cp); 
	model.alloc(npoints);
	view[0].alloc(npoints);
	view[1].alloc(npoints);
	
    }

    init();

    /************************************************************
     ** comienzo del algoritmo
     ***********************************************************/

    createModel_Manual();
    
    CvSize size = reader.image_size();
    cvNamedWindow(Model_SMAT[1], 1);
    cvMoveWindow(Model_SMAT[1], size.width+10,size.height+20);

    //show initial models above the image to make sure the model is working correctly
    //ShowModels(reader.img(), reader.cloneimages_color());

    view[0].calcPose();
    view[1].ChangeView(view[0]);

    CvSize displaysize;
    if (!TRACK)
    {
	//creación del modelo smat
	view[0].init_SMAT(reader.image_size(), filemodelsmat.c_str());
	view[1].init_SMAT(reader.image_size(), filemodelsmat.c_str());
	displaysize = cvSize(700,500);
    }
    else
    {
	displaysize = reader.image_size();
	displaysize.width +=40;
	displaysize.height = 700;
	view[0].init_track(reader.img(0));
	view[1].init_track(reader.img(1));
    }
    backup.alloc(4,reader.image_size(),reader.image_size(),displaysize,displaysize);

    view[0].getNotHiden();
    view[1].getNotHiden();

    //*************************************************************

    init_control();
    backup.copy_backup();
    frame2 = reader.head(0)->nframe;
    facedir_control(reader.img());
    view_0_aux = view[0];
    start();

    while(!quit)
    {
	//////////////////////////////
	synchro3->wait();

	//////////////////////////////
	synchro3->wait();
	facedir_control(reader.img());

	//////////////////////////////
	//synchro3->wait();

	backup.copy_backup();
	if (!quit)
	{
	    if (!reader.consumer())
		break;
	    frame2 = reader.head(0)->nframe;
	    if (frame2>3880 && frame2<4300)
		for (i=0; i<3; i++)
		{
		    if (!reader.consumer())
			break;
		    frame2 = reader.head(0)->nframe;
		}
	}
    }

    if (fout)
	fclose(fout);

    if (videofile)
	fclose(videofile);
    return 0;
}


void CFacedir::start()
{
    synchro3 = new boost::barrier(3);
    synchro2 = new boost::barrier(2);

    for (int i=0; i<2; i++)
	m_thread[i] = new boost::thread (boost::ref(*this), i);
}

void CFacedir::facedir_control(IplImage **images)
{
    int i;
    float incremento;
    float incrementopos;
    int modificado, modificando;
    modificado=0; 
    modificando=0;
    //float basura;

    incremento=0.5/180.0*CV_PI;
    incrementopos = 2;

    if (frame2 == endframe)
    {
	printf("\n------------------\nEnd frame %d reached\n----------\n", frame2);
	cvWaitKey(0);
	cvWaitKey(0);
	wait = 0;
    }

    do
    {
	printf(CRED "inliers L:%d, error:%f" CNORMAL "\n", view[0].inliers, view[0].error);
	printf(CGREEN "inliers R:%d, error:%f" CNORMAL "\n", view[1].inliers, view[1].error);
	printf("vect Left: ");
	print_arr_mul(view[0].R, 180.0/CV_PI, true, "%1.2f,");
	printf("translation Left: ");
	print_arr(view[0].T, true, "%3f, ");
	print_arr(view[0].maskoutliers, 1, "%3.0f, ");

	for (i=0; i<SHOW_VIEWS; i++)
	{
	    cvConvertImage(images[i], backup[i]);
	    dm.DrawModel(view[i], backup[i], view[i].maskoutliers);
	    dm.DrawModel3D(view[i], backup[i], view[i].maskoutliers);

	    cvShowImage(window[i], backup[i]);
	    cvShowImage(Model_SMAT[i], backup[2+i]);
	}
	
	modificando = 0;
	while ((key = bucle_cvWaitKey(&wait)) > 0)
	{
	    if (!(key & 0xFF0000))
	    {
		switch (key)
		{
		case 'w':
		case 'W':
		    key = -2;
		    break;
		case 'I':
		case 'i':
		    print_model_state(view[0].smat);	    
		    break;
		case  0xFF51: //BACK ARROW
		    //ir hacia atras en las imagenes a mostrar
		    backup.back_one();
		    printf("image frame diff: %d / %d\n", 
			   frame2 - backup.buffer_to_show, frame2); 
		    for (i=0; i<4; i++)
			cvShowImage(window[i], backup.img(i));
		    break;
		case  0xFF53: //NEXT ARROW
		    //ir hacia atras adelante en las imagenes a mostrar
		    backup.forward_one();
		    printf("image frame diff: %d / %d\n", 
			   frame2 - backup.buffer_to_show, frame2);
		    for (i=0; i<4; i++)
			cvShowImage(window[i], backup.img(i));
		    break;
		case 0xFF52: //UP
		    break;
		case 0xFF54: //DOWN
		    break;
		case 's':
		case 'S':
		    SaveModel(basefilename, reader.get_ROI());
		    break;
		case 'a':
		case 'A':
		    printf("Edit 3D Model\n");
		    createModel_Manual();
		    view[0].init_track(reader.img(0));
		    view[1].init_track(reader.img(1));
		default:
		    printf("Undefined key %d - (%X)\n", key, key);
		}
	    }
	    else
	    {
		switch(key)
		{
		    //SHIFT + ...
		case  0x1FF51: //left
		    *c += -incremento;
		    break;
		case  0x1FF53: //right
		    *c += incremento;
		    break;

		    //SHIFT
		case 0x1FF52: //UP
		    *z += -incrementopos;
		    break;
		case 0x1FF54: //DOWN
		    *z += incrementopos;
		    break;

		    //ctrl + ...
		case  0x4FF51: //left
		    *b += incremento;
		    break;
		case  0x4FF53: //right
		    *b += -incremento;
		    break;
		case 0x4FF52: //UP
		    *a += -incremento;
		    break;
		case 0x4FF54: //DOWN
		    *a += incremento;
		    break;

		    //ALT + ...
		case  0x8FF51: //left
		    *x += -incrementopos;
		    break;
		case  0x8FF53: //right
		    *x += incrementopos;
		    break;
		case 0x8FF52: //UP
		    *y += -incrementopos;
		    break;
		case 0x8FF54: //DOWN
		    *y += incrementopos;
		    break;
		case 0x40000 | 'p':
		case 0x40000 | 'P':
		    printf("Not Implemented.\n");
		break;
		default:
		    printf("Undefined key %d(%X)\n", key, key);
		}
		modificado = modificando = 1;
		key = 1;
		wait=0;
		break;
		if (key<=0)
		    break;
	    } //if (key & 0xFF0000)
	}  //while ((key = bucle_cvWaitKey(&wait)) > 0)
	if(key < 0)
	{
	    quit = true;
	}
	if (modificando)
	    view[1].ChangeView(view[0]);

    } while (modificando && key > 0);

    if (modificado)
    {
	modificado =0;
	view[1].ChangeView(view[0]);
	view[0].reproject();
	view[1].reproject();
	printf("-- modificado --\n");
    }

    if (frame > 0)
    {	
	if (save_video && key!=-1)
	{
	    //write results
	    if (videofile)
		fwrite(backup[0]->imageData, backup[0]->imageSize, 1, videofile);

	    fprintf(fout, "%d ", frame2);
	    view[0].fprintf(fout);
	    view[1].fprintf(fout);
	    fprintf(fout, "\n");
	    fflush(fout);

	    printf("---------------- saving frame %d --------------\n", frame);
	}
    }

    frame ++;
}
 

void CFacedir::init()
{
    if (iniframe < 0)
	iniframe=0;
 
    if(save_video)
    {
	std::string buffer = get_pathfile()+format("/video_%05d.raw", iniframe);
	videofile = fopen(buffer.c_str(), "w");
	buffer = get_pathfile()+format("/video_%05d.txt", iniframe);
	fout = fopen (buffer.c_str(), "w");
    }
}


void CFacedir::init_control()
{
    a = &view[0].vect->data.fl[0];
    b = &view[0].vect->data.fl[1];
    c = &view[0].vect->data.fl[2];

    x = &view[0].translation->data.fl[0];
    y = &view[0].translation->data.fl[1];
    z = &view[0].translation->data.fl[2];

    frame = 0;
    quit = false;
}

void CFacedir::readCalibration(std::string filename)
{
    //búsqueda del fichero de calibración y 
    //  leer los parámetros de calibración
    printf("fichero de\ncalibración: %s\n", filename.c_str());

    CvFileStorage* fs = cvOpenFileStorage( filename.c_str(), 0, CV_STORAGE_READ );
    calibration_params[0].Fundamental = (CvMat *) cvReadByName(fs, NULL, "Fundamental");
    calibration_params[0].R = (CvMat *) cvReadByName(fs, NULL, "R");
    calibration_params[0].T = (CvMat *) cvReadByName(fs, NULL, "T");
    calibration_params[0].KK = (CvMat *) cvReadByName(fs, NULL, "Fl");
    calibration_params[1].KK = (CvMat *) cvReadByName(fs, NULL, "Fr");
    
    cp.block_size = cvReadInt(cvGetFileNodeByName( fs, NULL, "block_size"), 19) ;
    cp.k_harris = cvReadReal(cvGetFileNodeByName( fs, NULL, "k_harris"), 0.2375);
    cp.min_distance = cvReadInt(cvGetFileNodeByName( fs, NULL, "min_distance"), 30);
    cp.quality_level_max=cvReadReal(cvGetFileNodeByName(fs, NULL, "quality_level_max"),0.006);
    cp.quality_level_min=cvReadReal(cvGetFileNodeByName(fs, NULL, "quality_level_min"),0.0000400);
    cvReleaseFileStorage( &fs );

    calibration_params[1].Fundamental = cvCloneMat(calibration_params[0].Fundamental);
    calibration_params[1].R = cvCloneMat(calibration_params[0].R);
    calibration_params[1].T = cvCloneMat(calibration_params[0].T);

    cvTranspose(calibration_params[1].R, calibration_params[1].R);
    cvMatMul(calibration_params[1].R, calibration_params[1].T, calibration_params[1].T);
    cvScale(calibration_params[1].T, calibration_params[1].T, -1);
    
    calibration_params[0].kk = calibration_params[0].KK->data.fl;
    calibration_params[1].kk = calibration_params[1].KK->data.fl;
       
    STEREO_PARAMS_t *tmp = (STEREO_PARAMS_t*)malloc(1*sizeof(STEREO_PARAMS_t));
    calibration_params[0].tmp = tmp;
    calibration_params[1].tmp = 0; //de momento indefinico.

    /*para las ecuaciones, (X,Y,Z) están referidos a la cámara izquierda.
      pero para operar con (ur, vr) y (u0r, v0r) hay que pasarlo a (X,Y,Z) de la derecha.
      PAra ello usamos la R y la T de la derecha.
    */
    tmp->rr = calibration_params[1].R->data.fl;
    tmp->tt = calibration_params[1].T->data.fl;

    tmp->fxR00 = calibration_params[1].kk[0*3+0]*tmp->rr[0*3+0];
    tmp->fxR01 = calibration_params[1].kk[0*3+0]*tmp->rr[0*3+1];
    tmp->fxR02 = calibration_params[1].kk[0*3+0]*tmp->rr[0*3+2];
    
    tmp->fyR10 = calibration_params[1].kk[1*3+1]*tmp->rr[1*3+0];
    tmp->fyR11 = calibration_params[1].kk[1*3+1]*tmp->rr[1*3+1];
    tmp->fyR12 = calibration_params[1].kk[1*3+1]*tmp->rr[1*3+2];
    
    tmp->fxT0 = calibration_params[1].kk[0*3+0]*tmp->tt[0];
    tmp->fyT1 = calibration_params[1].kk[1*3+1]*tmp->tt[1];

    tmp->M_A = cvCreateMat(4,3,CV_32F);
    tmp->V_B = cvCreateMat(4,1,CV_32F);

    cvmSet(tmp->M_A, 0,0, calibration_params[0].kk[0*3+0]  );
    cvmSet(tmp->M_A, 0,1, 0);
    cvmSet(tmp->M_A, 1,0, 0);
    cvmSet(tmp->M_A, 1,1, calibration_params[0].kk[1*3+1]  );

    cvmSet(tmp->V_B, 0,0, 0);
    cvmSet(tmp->V_B, 1,0, 0);
}



void CFacedir::ShowModels(IplImage ** images, IplImage ** cloneimages)
{
    char windows2[4][100] = {"Show Model3D Left", "Show Model3D Right", "", ""};
    strcpy(windows2[2], window[2]);
    strcpy(windows2[3], window[3]);

    cvNamedWindow(windows2[0], 1);
    cvNamedWindow(windows2[1], 1);
    cvNamedWindow(windows2[2], 1);
    cvNamedWindow(windows2[3], 1);

    view[0].calcPose();
    view[0].getNotHiden();
    dm.DrawModel3D(view[0], cloneimages[0]);
    cvShowImage(windows2[0], cloneimages[0]);

    view[1].ChangeView(view[0]);
    dm.DrawModel3D(view[1], cloneimages[1]);
    cvShowImage(windows2[1], cloneimages[1]);
 
    cvConvertImage(images[0], cloneimages[0]);
    cvConvertImage(images[1], cloneimages[1]);
    view[1].calcPose();
    view[1].getNotHiden();
    dm.DrawModel3D(view[1], cloneimages[1], NULL, -1, CV_RGB(100,200,255));
    cvShowImage(windows2[3], cloneimages[1]);

    view[0].ChangeView(view[1]);
    dm.DrawModel3D(view[0], cloneimages[0], NULL, -1, CV_RGB(100,200,255));
    cvShowImage(windows2[2], cloneimages[0]);

    cvWaitKey(0);
    cvDestroyWindow(windows2[0]);
    cvDestroyWindow(windows2[1]);
    //this is only to allow opencv destroy the windows.
    cvWaitKey(10);
}


bool CFacedir::ReadModel(std::string basename, CvRect roi)
{
    std::string buffermodel;
    std::string bufferview;
    int i;

    buffermodel = basename + format(".dat3d");
    if (frame2)
	bufferview = basename + format(".dat2d.%d", frame2);
    else
	bufferview = basename + format(".dat2d");

    model.ReadModelFromFile(buffermodel);
    for (i=0; i<nviews; i++)
    {
	view[i].ReadModelFromFile(bufferview, i, roi);
	view[i].setmask_show();
	//cvSubS(&view[i].points2d2, cvScalar(0,(700-480)/2), &view[i].points2d2);
    }

    if (model.npoints == 0)
	return false;

    if (model.npoints != view[0].npoints || model.npoints != view[1].npoints)
    {
	int i, j;
	for (i=0; i<2; i++)
	{
	    for (j=view[i].npoints; j<model.npoints; j++)
	    {
		view[i].addpoint();
		cvSetReal1D(view[i].mask_show, j, 0);
	    }
	}
    }
    return true;
}

bool CFacedir::SaveModel(std::string basename, CvRect roi)
{
    view[0].reproject();
    view[1].reproject();
    std::string buffermodel;
    std::string bufferview;

    buffermodel = basename + format(".dat3d");
    if (frame2)
	bufferview = basename + format(".dat2d.%d", frame2);
    else
	bufferview = basename + format(".dat2d");

    CvFileStorage * fs = cvOpenFileStorage( bufferview.c_str(), 0, CV_STORAGE_WRITE );
    view[0].SaveModel(fs, 0, roi);
    view[1].SaveModel(fs, 1, roi);
    cvReleaseFileStorage(&fs);
    fs = cvOpenFileStorage( buffermodel.c_str(), 0, CV_STORAGE_WRITE );
    model.SaveModel(fs);

    cvReleaseFileStorage(&fs);

    printf("Model saved to %s\n", buffermodel.c_str());
    printf("Views saved to %s\n", bufferview.c_str());
    return true;
}


std::string CFacedir::get_pathfile()
{
    return basefilename.substr(0, basefilename.find_last_of('/')); 
}
